﻿#include "FITKAbaTransform.h"
#include "FITK_Kernel/FITKCore/FITKVec3D.h"
#include "FITK_Kernel/FITKCore/FITKVec3DAlg.h"

namespace Interface
{
    FITKAbaTransform::FITKAbaTransform(FCSysType sysType)
        :FITKCoordinateSystem(sysType)
    {
        //创建点
        _point1 = new Core::FITKPoint(0, 0, 0);
        _point2 = new Core::FITKPoint(0, 0, 0);
    }


    FITKAbaTransform::~FITKAbaTransform()
    {
        //释放点
        if (_point1) delete _point1;
        if (_point2) delete _point2;
    }

    void FITKAbaTransform::setTransform(double* p1, double* p2)
    {
        //设置参数
        if(p1)
            _point1->setCoor(p1[0], p1[1], p1[2]);
        if(p2)
            _point2->setCoor(p2[0], p2[1], p2[2]);

        this->update();
    }

    void FITKAbaTransform::getTransform(double* p1, double* p2)
    {
        if (p1 == nullptr || p2 == nullptr) return;

        if (_sysType == FITKCoordinateSystem::FCSCartesian)
        {
            //初始化为坐标原点
            this->getOrigin(p1);
            this->getOrigin(p2);
            double d[3] = { 0,0,0 };
            //第一轴方向，偏置
            this->getDirection1(d);
            for (int i = 0; i < 3; ++i)
                p1[i] += d[i];
            //第二轴线方向，并偏置
            this->getDirection2(d);
            for (int i = 0; i < 3; ++i)
                p2[i] += d[i];

        }
        else  //柱坐标与球坐标
        {
            //第一点为坐标原点
            this->getOrigin(p1);
            //第三轴线方向，并偏置
            this->getDirection3(p2);
            for (int i = 0; i < 3; ++i)
                p2[i] += p1[i];
        }
    }

   

    void FITKAbaTransform::update()
    {
        //根据类型创建坐标系
        switch (_sysType)
        {
        case Interface::FITKCoordinateSystem::FCSCartesian:
            this->createCartesianSystem();
            break;
        case Interface::FITKCoordinateSystem::FCSCylinder:
            this->createCylinderSystem();
            break;
        case Interface::FITKCoordinateSystem::FCSSphere:
            this->createSphereSystem();
            break;
        default: break;
        }
    }

    void FITKAbaTransform::get3DirectionAtPoint(const double* pxyz, double* dirs)
    {
        if (_sysType == FITKCoordinateSystem::FCSCartesian)
        {
            FITKCoordinateSystem::get3DirectionAtPoint(pxyz, dirs);
        }
        else
        {
            Core::FITKPoint p(pxyz[0], pxyz[1], pxyz[2]);
            Core::FITKVec3D td1 = Subtract(p, *_point1);
            Core::FITKVec3D d3 = Subtract(*_point2, *_point1);
            Core::FITKVec3D d2 = CrossProduct(d3, td1);
            Core::FITKVec3D d1 = CrossProduct(d2, d3);
            //赋值
            for (int i = 0; i < 3; ++i)
                dirs[i] = d1[i];
            for (int i = 0; i < 3; ++i)
                dirs[i + 3] = d2[i];
            for (int i = 0; i < 3; ++i)
                dirs[i + 6] = d3[i];
        }
    }

    void FITKAbaTransform::createCartesianSystem()
    {
        //笛卡尔坐标系
        this->setPoint1(_point1->x(), _point1->y(), _point1->z());
        this->setPoint2(_point2->x(), _point2->y(), _point2->z());
    }

    void FITKAbaTransform::createCylinderSystem()
    {
        //z方向
        Core::FITKVec3D zD = Core::Subtract(*_point2, *_point1);
        if (zD.getNorm() < 1e-9) return;

        //假设第二轴线是x
        Core::FITKVec3D d2(1, 0, 0);
        //第三轴线与x平行则选取y轴
        double angle = AngleBetweenVec(zD, d2);
        if (angle < 1e-4 || fabs(angle - FITK_PI) < 1e-4)
            d2.setCoor(0, 1, 0);

        Core::FITKVec3D d1 = CrossProduct(d2, zD);
        d2 = CrossProduct(zD, d1);
        //坐标原点
        this->setOrigin(_point1->x(), _point1->y(), _point1->z());
        this->setPoint1(_point1->x() + d1[0], _point1->y() + d1[1], _point1->z() + d1[2]);
        this->setPoint2(_point1->x() + d2[0], _point1->y() + d2[1], _point1->z() + d2[2]);

    }

    void FITKAbaTransform::createSphereSystem()
    {
        // z方向
        Core::FITKVec3D zD = Core::Subtract(*_point2, *_point1);
        if (zD.getNorm() < 1e-9) return;

        //假设第二轴线是x
        Core::FITKVec3D d2(1, 0, 0);
        //第三轴线与x平行则选取y轴
        double angle = AngleBetweenVec(zD, d2);
        if (angle < 1e-4 || fabs(angle - FITK_PI) < 1e-4)
            d2.setCoor(0, 1, 0);

        Core::FITKVec3D d1 = CrossProduct(d2, zD);
        d2 = CrossProduct(zD, d1);
        //坐标原点
        this->setOrigin(_point1->x(), _point1->y(), _point1->z());
        //第一第二坐标轴
        this->setPoint1(_point1->x() + d1[0], _point1->y() + d1[1], _point1->z() + d1[2]);
        this->setPoint2(_point1->x() + d2[0], _point1->y() + d2[1], _point1->z() + d2[2]);
    }

}


